/*
 * Copyright (c) 2022, www.lgmgim.cn
 * All rights reserved.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *  * Neither the name of the University of California, Berkeley nor the
 *    names of its contributors may be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
 * @file kacanopen.cpp
 * @author gc
 * @date Sep 28, 2024
 * @brief kacanopen node
 * Contact: congcong.guan@lgmgim.cn
 */


#include "kacanopen_new.h"
#include "sdo_error.h"
#include <shared_parameters/shared_params_interface.hpp>

#define CANOPEN_PARAM_PATH "CANopen"
#define DEVICE_JOINT_PATH "joint"

KaCanopen::KaCanopen()
{
}

KaCanopen::~KaCanopen()
{
	for (size_t i=0; i<masters_.size(); i++) {
		masters_[i]->sync_thread_running = false;
		masters_[i]->node_guard_threads_running = false;
		masters_[i]->sync_thread.detach();
		for (size_t j=0; j<masters_[i]->node_guard_threads.size();j++) {
			masters_[i]->node_guard_threads[j].detach();
		}

		delete masters_[i]->master;
		masters_[i]->master = nullptr;
		delete masters_[i];
		masters_[i] = nullptr;
	}

	masters_.resize(0);
}

bool KaCanopen::read_default_config_file()
{
	dev_default_config_path_ = std::string(getenv("COLCON_PREFIX_PATH")) + SHARE_INSTALLED_PATH
									+ std::string("/dev_default_config.yaml");

	std::fstream dev_default_fin = std::fstream(dev_default_config_path_.c_str(), std::ios::in);
    if (dev_default_fin.fail()) {
        RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "default config file: Could not open %s.", dev_default_config_path_.c_str());
        return false;
    }

    try {
        dev_default_node_ = YAML::Load(dev_default_fin);
        dev_default_fin.close();
        if (!dev_default_node_) {
            RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "Read %s Faild. data is not yaml format", dev_default_config_path_.c_str());
            return false;
        }
    } 
    catch (const YAML::Exception &e) {
        RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "load File format error %s,%s.", dev_default_config_path_.c_str(), e.what());
        return false;
    } 
    catch (...) {
        RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "unkown File format error %s", dev_default_config_path_.c_str());
        return false;
    }

    return true;
}


agv_msgs::msg::CANopenNodeStatus KaCanopen::init_node_status(kaco::Master* master, std::string busname)
{
	agv_msgs::msg::CANopenNodeStatus cns;
	cns.node_status.resize(0);

	if (can_sys_info_.busInfos[busname].deviceCount > 0) {
		for (auto device_name : can_sys_info_.busInfos[busname].devices) {
			agv_msgs::msg::CANopenNodeState ns;
			ns.busname = busname;
			ns.communication_status = std::string("NOTFOUND");
			ns.running_status = std::string("UNINITIALIZED");
			
			if (bridge_->get_parameter(busname + "/device_info/" + device_name + "/node_id", ns.nodeid)) {
				bridge_->get_parameter_or(busname + "/device_info/" + device_name + "/device_type", ns.type, -1);
			}

			std::string manu_name = get_manu_name_by_devicetype(ns.type);
			RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] device[%s] type[%d] get manu name[%s]",
				busname.c_str(), device_name.c_str(), ns.type, manu_name.c_str());
			if (dev_default_node_[manu_name]["standard"]) {
				if (dev_default_node_[manu_name]["standard"].as<bool>()) {
		    		RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "get standard device name: %s", device_name.c_str());	
    			}
    			else {
					RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "set communication status to CONNECTED");
					ns.communication_status = std::string("CONNECTED");
					master->device_alive_callback(ns.nodeid);
	    		}
	    	}
	    	else {
	    		RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "manu[%s] cannot get param[standard]", manu_name.c_str());
	    	}

			cns.node_status.push_back(ns);
		}
	}

	return cns;
}

void KaCanopen::process_can_info(const std::string& path) 
{
    size_t first_slash = path.find('/');
    if (first_slash == std::string::npos) return;

    std::string bus_name = path.substr(0, first_slash);
    
    std::string device_info_prefix = bus_name + "/device_info/";
    if (path.find(device_info_prefix) == 0) {
        size_t device_start = device_info_prefix.length();
        size_t device_end = path.find('/', device_start);
        if (device_end != std::string::npos) {
            std::string device_name = path.substr(device_start, device_end - device_start);
            
            auto& bus_info = can_sys_info_.busInfos[bus_name];
           
            if (std::find(bus_info.devices.begin(), bus_info.devices.end(), device_name) 
                	== bus_info.devices.end()) {
                bus_info.devices.push_back(device_name);
                bus_info.deviceCount++;
            }
        }
    }
}

bool KaCanopen::init_param()
{
    std::set<std::string> uniqueCANs;
   
	if (!SharedParamsInterface::initialize()) {
        RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "Failed to initialize shared memory interface");
        return false;
    }

    try {
            auto params = SharedParamsInterface::getNodeParams(CANOPEN_PARAM_PATH);
            std::cout << "Get \"" << CANOPEN_PARAM_PATH << "\" params:";
            for (const auto& [name, value] : params) {
                switch(value.type) {
		            case SharedParamsInterface::ParamValue::Type::INT:
		                bridge_->declare_parameter(name, value.getValue<int>());
		                break;
		            case SharedParamsInterface::ParamValue::Type::DOUBLE:
		                bridge_->declare_parameter(name, value.getValue<double>());
		                break;
		            case SharedParamsInterface::ParamValue::Type::STRING:
		                bridge_->declare_parameter(name, value.getValue<std::string>());
		                break;
		            case SharedParamsInterface::ParamValue::Type::BOOL:
		                bridge_->declare_parameter(name, value.getValue<bool>());
		                break;
		            default:
		                RCLCPP_WARN(bridge_->get_logger(), 
		                           "Unsupported parameter type for %s", name.c_str());
        		}

        		process_can_info(name);

        		size_t pos = name.find('/');
	            if (pos != std::string::npos) {
	                std::string device = name.substr(0, pos);
	                uniqueCANs.insert(device);
	            }   
        	}

            std::cout << "success." << std::endl;

    } catch (const std::exception& e) {
        RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "Error reading parameters: %s", e.what());
        return false;
    }
	
	can_sys_info_.busCount = uniqueCANs.size();

	for (auto& [bus_name, bus_info] : can_sys_info_.busInfos) {
    	std::sort(bus_info.devices.begin(), bus_info.devices.end());
	}

	RCLCPP_INFO(bridge_->get_logger(), "CAN System Statistics:");
	RCLCPP_INFO(bridge_->get_logger(), "Total CAN buses: %d", can_sys_info_.busCount);
	for (const auto& [bus_name, bus_info] : can_sys_info_.busInfos) {
	    RCLCPP_INFO(bridge_->get_logger(), "%s: %d devices", 
	                bus_name.c_str(), bus_info.deviceCount);
	    std::string devices;
	    for (const auto& device : bus_info.devices) {
	        devices += device + " ";
	    }
	    RCLCPP_INFO(bridge_->get_logger(), "Devices: %s", devices.c_str());
	}

	try {
            auto params = SharedParamsInterface::getNodeParams(DEVICE_JOINT_PATH);
            std::cout << "Get \"" << DEVICE_JOINT_PATH << "\" params:";
            std::regex nodeIdPattern("(\\w+_joint)/nodeId");
            for (const auto& [name, value] : params) {
                switch(value.type) {
		            case SharedParamsInterface::ParamValue::Type::INT:
		                bridge_->declare_parameter(name, value.getValue<int>());
		                break;
		            case SharedParamsInterface::ParamValue::Type::DOUBLE:
		                bridge_->declare_parameter(name, value.getValue<double>());
		                break;
		            case SharedParamsInterface::ParamValue::Type::STRING:
		                bridge_->declare_parameter(name, value.getValue<std::string>());
		                break;
		            case SharedParamsInterface::ParamValue::Type::BOOL:
		                bridge_->declare_parameter(name, value.getValue<bool>());
		                break;
		            default:
		                RCLCPP_WARN(bridge_->get_logger(), 
		                           "Unsupported parameter type for %s", name.c_str());
        		}

        		
	        	std::string line = std::regex_replace(name, std::regex("^\\s+"), "");
	        	std::smatch matches;
		        if (std::regex_search(line, matches, nodeIdPattern)) {
		            joint_names_.push_back(matches[1]);
		        } 
        	}
        	std::cout << "success." << std::endl;
    } catch (const std::exception& e) {
        RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "Error reading parameters: %s", e.what());
        return false;
    }

	for (const auto& busname : uniqueCANs) {
		RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "get param from CANopen: %s", busname.c_str());
	
        if (busname.compare(0, 3, "can") == 0) {
        	RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "get busname: %s", busname.c_str());

        	int baudrate = 500;
        	if (bridge_->get_parameter(busname + "/baudrate", baudrate)) {
        		RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "get baudrate: %d", baudrate);

				MasterCollector* mc = new MasterCollector();
				mc->master = new kaco::Master();
				mc->busname = busname;
				mc->baudrate = std::to_string(baudrate) + "k";
				mc->node_guard_threads_running = false;
				mc->node_guard_threads.resize(0);
				mc->node_status = init_node_status(mc->master, busname);
				mc->futures.clear();
	            bridge_->get_parameter_or(busname + "/reset_mask", mc->reset_mask, false);

	            masters_.emplace_back(mc);
	        }
			else {
				RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "bus[%s] baudrate not defined.", busname.c_str());
				return false;
			}
		}
	}

	return true;
}


bool KaCanopen::start_canopen()
{
	for (auto& mc : masters_) {
		if (!mc->master->start(mc->busname, mc->baudrate)) {
	    	RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "Starting master at %s failed.", mc->busname.c_str());
			return false;
		}
		if (!mc->reset_mask)
			mc->master->core.nmt.reset_all_nodes();
		else
			RCLCPP_INFO_STREAM(rclcpp::get_logger("Kacanopen"), "reset mask");
	}

	return true;
}


bool KaCanopen::init(std::shared_ptr<kaco::Bridge> bridge)
{
	auto now = std::chrono::system_clock::now();
	node_start_time_ = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count() / 1000.0;

	bridge_ = bridge;
    event_pub_ = bridge_->create_publisher<agv_msgs::msg::AGVEvent>("/agvevent", 1);
    node_status_pub_ = bridge_->create_publisher<agv_msgs::msg::CANopenNodeStatus>("/canopen_node_status", 1);

	set_entry_srv_ = bridge_->create_service<agv_srvs::srv::SetCanopenEntry>(
		"/set_canopen_entry", 
		std::bind(&KaCanopen::set_canopen_entry_callback, this, std::placeholders::_1, std::placeholders::_2));

    drivers_info_sub_  = bridge_->create_subscription<agv_msgs::msg::DriversInfo>(
    	"/driversInfo", 10, 
    	std::bind(&KaCanopen::drivers_info_callback, this, std::placeholders::_1));

	timeout_ = bridge_->get_parameter_or("timeout", 1.0);

	ep_.init(bridge_->shared_from_this());
	
	if (!read_default_config_file())
		return false;

	masters_.resize(0);

    if (!init_param()) {
    	return false;
    }

    if (!start_canopen()) {
    	return false;
    }
    
	assert(masters_.size() > 0);
	for (size_t i=0; i<masters_.size(); i++) {
		masters_[i]->sync_thread_running = true;
		masters_[i]->sync_thread = std::thread(&KaCanopen::sync_thread_func, this, std::ref(masters_[i]->sync_thread_running), masters_[i]->master);
	}
	
	std::this_thread::sleep_for(std::chrono::seconds(1));
	return true;
}


void KaCanopen::update()
{
	cns_.node_status.clear();
	cns_.node_status.resize(0);
	
	for (auto& mc : masters_) {
		for (auto& cns : mc->node_status.node_status) {
			if (cns.communication_status == std::string("NOTFOUND")) {
				mc->master->core.nmt.discover_node(cns.nodeid);
				RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "try to discover %s node[%d] ...", mc->busname.c_str(), cns.nodeid);
				// std::this_thread::sleep_for(std::chrono::milliseconds(50));
				if (mc->master->is_node_existed(cns.nodeid))
					cns.running_status = std::string("UNINITIALIZED");
			}

			if (cns.running_status == std::string("UNINITIALIZED")) {
				if (mc->futures.find(cns.nodeid) == mc->futures.end()) {
		            mc->futures[cns.nodeid] = std::async(std::launch::async, std::bind(&KaCanopen::init_node, this, 
		            	std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), mc, cns.nodeid, cns.type);
					cns.running_status = std::string("INITIALIZING");
				}
			}

			if (cns.running_status == std::string("INITIALIZING")) {
				if (mc->futures.find(cns.nodeid) != mc->futures.end()) {
					std::future_status status = mc->futures[cns.nodeid].wait_for(std::chrono::seconds(0));

					if (status == std::future_status::ready) {
						bool result = false;
						try {
							result = mc->futures[cns.nodeid].get();
						}
						catch (const std::future_error& e) {
	                        RCLCPP_ERROR_STREAM(rclcpp::get_logger("Kacanopenr"), "future for node[" << cns.nodeid << "] get result error: " << e.what());
	                        result = false;
                    	}

                    	cns.running_status = result ? std::string("RUNNING") : std::string("UNINITIALIZED");
                    	mc->futures.erase(cns.nodeid);
					}
				}
				else {
					cns.running_status = std::string("UNINITIALIZED");
				}
			}
		}

		
		cns_.node_status.insert(cns_.node_status.end(), mc->node_status.node_status.begin(), mc->node_status.node_status.end());
	}

	check_devices_alive_status();
	node_status_pub_->publish(cns_);
}

std::string KaCanopen::get_device_name_by_nodeid(std::string busname, int nodeid)
{
	int id = -1;
	std::string param_prefix = busname + "/device_info";
	if (can_sys_info_.busInfos[busname].deviceCount > 0) {
		for (auto device_name : can_sys_info_.busInfos[busname].devices) {
			if (bridge_->get_parameter(param_prefix + "/" + device_name + "/node_id", id)) {
				if (id == nodeid)
					return device_name;
			}
		}
	}

	return std::string("");
}


std::vector<ParamNodeInfo> KaCanopen::get_canopen_config_node_list(std::string busname)
{
	std::vector<ParamNodeInfo> node_info_list;
	node_info_list.resize(0);

	std::string device_param_prefix = busname + "/device_info";
	std::string joint_param_prefix = "/joint";

	int id = -1;
	int type = -1;
	if (can_sys_info_.busInfos[busname].deviceCount > 0) {
		for (auto device_name : can_sys_info_.busInfos[busname].devices) {
			ParamNodeInfo pni;
			if (bridge_->get_parameter(device_param_prefix + "/" + device_name + "/node_id", id)) {
				if (bridge_->get_parameter(device_param_prefix + "/" + device_name + "/device_type", type)) {
					pni.manu_name = get_manu_name_by_devicetype(type);
				}
				else {
					pni.manu_name = std::string("");
				}

				pni.node_id = id;
				pni.device_name = device_name;
				pni.joint_name = std::string("");

				int joint_nodeId = -1;
				for (auto joint_name : joint_names_) {
					if (bridge_->get_parameter(joint_param_prefix + "/" + joint_name + "/nodeId", joint_nodeId)) {
						if (joint_nodeId == id) {
							if (pni.joint_name.length() == 0) {
								pni.joint_name = joint_name;
							}
							else {
								pni.joint_name += "," + joint_name;
							}
						}
					}
				}

				node_info_list.push_back(pni);
			}
		}
	}

	return node_info_list;
}

std::string KaCanopen::get_manu_name_by_devicetype(int type) 
{
	std::string manu_name;
	switch (type) {
		case DeviceManufacturer::KINCO:
			manu_name = "KINCO";
			break;
		case DeviceManufacturer::ZAPI:
			manu_name = "ZAPI";
			break;
		case DeviceManufacturer::CURTIS:
			manu_name = "CURTIS";
			break;
		case DeviceManufacturer::NEWSTART:
			manu_name = "NEWSTART";
			break;
		case DeviceManufacturer::YUFENG_CURTIS:
			manu_name = "YUFENG_CURTIS";
			break;
		case DeviceManufacturer::KEYA:
			manu_name = "KEYA";
			break;
		case DeviceManufacturer::NEWSTART_MAIRUI:
			manu_name = "NEWSTART_MAIRUI";
			break;
		case DeviceManufacturer::ENCODER_406:
			manu_name = "ENCODER_406";
			break;
		case DeviceManufacturer::ENCODER_AIDIKE:
			manu_name = "ENCODER_AIDIKE";
			break;
		case DeviceManufacturer::CODE_READ_CAMERA:
			manu_name = "CODE_READ_CAMERA";
			break;
		case DeviceManufacturer::IMU:
			manu_name = "IMU";
			break;
		case DeviceManufacturer::SICK_MAG_IMU:
			manu_name = "SICK_MAG_IMU";
			break;
		case DeviceManufacturer::TITAN_BATTERY:
			manu_name = "TITAN_BATTERY";
			break;
		case DeviceManufacturer::JUYUN_BATTERY:
			manu_name = "JUYUN_BATTERY";
			break;
		case DeviceManufacturer::HINSON_MAG:
			manu_name = "HINSON_MAG";
			break;
		case DeviceManufacturer::HINSON_RFID:
			manu_name = "HINSON_RFID";
			break;
		case DeviceManufacturer::EDGE_IO_BOARD:
			manu_name = "EDGE_IO_BOARD";
			break;
		case DeviceManufacturer::BECKHOFF_IO_3_3:
			manu_name = "BECKHOFF_IO_3_3";
			break;
		case DeviceManufacturer::BECKHOFF_IO_4_4:
			manu_name = "BECKHOFF_IO_4_4";
			break;
		case DeviceManufacturer::CAN_SPEAKER:
			manu_name = "CAN_SPEAKER";
			break;
		default:
			RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "manufacturer[%d] not defined in %s", type, dev_default_config_path_.c_str());
			manu_name = "";
			break;
	}

	return manu_name;
}


bool KaCanopen::init_node(MasterCollector* mc, uint8_t nodeid, int type)
{
	RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] init node[%d]", mc->busname.c_str(), nodeid);
	mc->master->core.nmt.send_nmt_message(nodeid, kaco::NMT::Command::enter_preoperational);
	std::this_thread::sleep_for(std::chrono::milliseconds(500));
	// i: device index   j: pdo channel index   k: pdo mapping object index
	try {
		kaco::Device& device_temp = mc->master->get_device_by_nodeid(nodeid);
	}
	catch (...) {
		RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "device nodeid[%d] not found in %s", nodeid, mc->busname.c_str());
		return false;
	}
	
	kaco::Device& device = mc->master->get_device_by_nodeid(nodeid);
	device.set_device_type(type);
	std::string device_name = get_device_name_by_nodeid(mc->busname, device.get_node_id());
	std::string device_to_search = "/CANopen/" + mc->busname + "/device_info/" + device_name;
	if (device_name == "") {
		RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "%s cannot find device nodeid[%d] in parameter server", mc->busname.c_str(), device.get_node_id());
		return false;
	}
		
	YAML::Node m_node;
	std::string type_name = get_manu_name_by_devicetype(type);
	if (dev_default_node_[type_name]) {
		m_node = dev_default_node_[type_name];
		RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] get type: %s", mc->busname.c_str(), nodeid, type_name.c_str());
	}
	else {
		RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "%s not defined in %s", type_name.c_str(), dev_default_config_path_.c_str());
		return false;
	}

	if (m_node["edsFile"]) {
		std::string eds_path  = std::string(getenv("COLCON_PREFIX_PATH")) + SHARE_INSTALLED_PATH 
			+ std::string("/") + m_node["edsFile"].as<std::string>();
		RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] load eds file: %s", mc->busname.c_str(), nodeid, eds_path.c_str());
		device.load_dictionary_from_eds(eds_path);
	}
	else {
		RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] eds file undefined.", mc->busname.c_str(), nodeid);
		return false;
	}

		
	int nodeguarding_mode = -1;
	if (bridge_->get_parameter(device_to_search + "/nodeGuarding_mode", nodeguarding_mode)) {
		switch (nodeguarding_mode) {
			case NodeGuardingMode::NoUse:
				break;

			case NodeGuardingMode::NodeGuarding:
			{
				int nodeguarding_time = -1;
				if (bridge_->get_parameter(device_to_search + "/nodeGuarding_time", nodeguarding_time)) {
					uint16_t guard_time = nodeguarding_time;
					device.set_entry("guard_time", guard_time, kaco::WriteAccessMethod::sdo);
				}

				int life_time_factor_temp = -1;
				if (bridge_->get_parameter(device_to_search + "/life_time_factor", life_time_factor_temp)) {
					uint8_t life_time_factor = life_time_factor_temp;
					device.set_entry("life_time_factor", life_time_factor, kaco::WriteAccessMethod::sdo);
				}


				device.set_entry("abort_connection_mode", (int16_t)0x1, kaco::WriteAccessMethod::sdo);
				RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] set nodeGuard time %d", mc->busname.c_str(), nodeid, nodeguarding_time);
				mc->node_guard_threads_running = true;
				mc->node_guard_threads.emplace_back(std::thread(&KaCanopen::node_guard_thread_func, this, 
						std::ref(mc->node_guard_threads_running), mc->master, device.get_node_id(), nodeguarding_time));
			}
				break;

			case NodeGuardingMode::Heartbeat:
				// TODO
				break;

			default:
				break;
		}
	}
	else {
		RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] has no node guarding config, skipping", mc->busname.c_str(), nodeid);
	}


	if (m_node["pdoRawData"] && m_node["pdoRawData"].as<bool>()) {
		RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "%s enable pdo raw data.", device_name.c_str());
		device.add_entry((uint16_t)0x7000, (uint8_t)0x01, "pdo_raw_data_0", kaco::Type::uint64, kaco::AccessType::read_write);
		device.add_entry((uint16_t)0x7000, (uint8_t)0x02, "pdo_raw_data_1", kaco::Type::uint64, kaco::AccessType::read_write);
		device.add_entry((uint16_t)0x7000, (uint8_t)0x03, "pdo_raw_data_2", kaco::Type::uint64, kaco::AccessType::read_write);
		device.add_entry((uint16_t)0x7000, (uint8_t)0x04, "pdo_raw_data_3", kaco::Type::uint64, kaco::AccessType::read_write);					
		
		set_entry_bridge(device, mc->busname, "pdo_raw_data_0", BridgeType::publisher, 50);
		set_entry_bridge(device, mc->busname, "pdo_raw_data_1", BridgeType::publisher, 50);
		set_entry_bridge(device, mc->busname, "pdo_raw_data_2", BridgeType::publisher, 50);
		set_entry_bridge(device, mc->busname, "pdo_raw_data_3", BridgeType::publisher, 50);

		device.add_receive_pdo_mapping(0x180 + device.get_node_id(), "pdo_raw_data_0", (uint8_t)0x0);
		device.add_receive_pdo_mapping(0x280 + device.get_node_id(), "pdo_raw_data_1", (uint8_t)0x0);
		device.add_receive_pdo_mapping(0x380 + device.get_node_id(), "pdo_raw_data_2", (uint8_t)0x0);
		device.add_receive_pdo_mapping(0x480 + device.get_node_id(), "pdo_raw_data_3", (uint8_t)0x0);
	}

	if (m_node["entryTimestamp"]) {
		std::string entry_name = m_node["entryTimestamp"].as<std::string>();
		device.set_timestamp_entry_name(entry_name);
		set_entry_bridge(device, mc->busname, entry_name, BridgeType::publisher_timestamp, 100);
	}
	
	// 4 pdo channel at max
	for (size_t j=0; j<4; j++) {
		std::string tpdo_name = std::string("tpdoMappingCfg") + std::to_string(j);
		std::string rpdo_name = std::string("rpdoMappingCfg") + std::to_string(j);

		std::string tpdo_interval_name = std::string("tpdoInterval") + std::to_string(j);
		int tpdo_interval = 1;
		if (m_node[tpdo_interval_name]) {
			tpdo_interval = m_node[tpdo_interval_name].as<int>();
			tpdo_interval = tpdo_interval < 1 ? 1 : tpdo_interval;
			RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] get %s interval: %d", mc->busname.c_str(), nodeid, tpdo_interval_name.c_str(), tpdo_interval);
		}


		uint16_t tpdo_cob_id = 0x180 + 0x100*j + device.get_node_id();
		uint16_t rpdo_cob_id = 0x200 + 0x100*j + device.get_node_id();


		if (m_node[tpdo_name]) {
			RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] set tpdo: %s", mc->busname.c_str(), nodeid, tpdo_name.c_str());
			YAML::Node pdo_node = m_node[tpdo_name];
			uint8_t offset = 0;
			
			
			if (m_node["setPdoMapping"] && m_node["setPdoMapping"].as<bool>()) {
				// cob id
				device.set_entry(0x1800 + j, (uint8_t)0x1, 0x80000000 + (uint32_t)tpdo_cob_id, 0, kaco::WriteAccessMethod::sdo);
				// entry num
				device.set_entry(0x1A00 + j, (uint8_t)0x0, (uint8_t)0x0, 0, kaco::WriteAccessMethod::sdo);
				// transmission type
				RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] set tpdo[0x%x] interval %d", mc->busname.c_str(), nodeid, 0x1800 + j, tpdo_interval);
				device.set_entry(0x1800 + j, (uint8_t)0x2, (uint8_t)tpdo_interval, 0, kaco::WriteAccessMethod::sdo);
				// forbit time
				device.set_entry(0x1800 + j, (uint8_t)0x3, (uint16_t)0x05, 0, kaco::WriteAccessMethod::sdo);
			}

			for (size_t k=0; k<pdo_node.size(); k++) {
				uint32_t 	value 		= pdo_node[k].as<int>();
				uint16_t 	index 		= (value >> 16) & 0xFFFF;
				uint8_t 	sub_index 	= (value >> 8) & 0xFF;
				uint8_t 	len 		= (value & 0xFF) >> 3;

				if (!device.has_entry(index, sub_index)) {
					char c_name[32];
					sprintf(c_name, "%x%02d", index, sub_index);

					switch(len) {
						case 1:
							device.add_entry(index, sub_index, "entry_" + std::string(c_name), kaco::Type::uint8, kaco::AccessType::read_write);
							break;
						case 2:
							device.add_entry(index, sub_index, "entry_" + std::string(c_name), kaco::Type::uint16, kaco::AccessType::read_write);
							break;
						case 4:
							device.add_entry(index, sub_index, "entry_" + std::string(c_name), kaco::Type::uint32, kaco::AccessType::read_write);
							break;
						case 8:
							device.add_entry(index, sub_index, "entry_" + std::string(c_name), kaco::Type::uint64, kaco::AccessType::read_write);
							break;
						default:
							device.add_entry(index, sub_index, "entry_" + std::string(c_name), kaco::Type::uint64, kaco::AccessType::read_write);
							break;
					}

					RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] add unexist entry index[0x%x] sub_index[%d]  name: entry_%s", 
						mc->busname.c_str(), nodeid, index, sub_index, c_name);
				}

				std::string entry_name 	= device.get_entry_name(index, sub_index);
				
				if (m_node["setPdoMapping"] && m_node["setPdoMapping"].as<bool>()) {
					RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] set_tpdo_mapping  index[%x] sub_index[%x] value[%x]", 
						mc->busname.c_str(), nodeid, 0x1A00+j, k+1, value);
					device.set_entry(0x1A00 + j, k+1, value, 0, kaco::WriteAccessMethod::sdo);
				}

				if (!m_node["pdoRawData"] || !m_node["pdoRawData"].as<bool>()) {
					RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] add_receive_pdo_mapping:  cobid[%x] name[%s] index[%x] sub_index[%x] offset[%x]",
							mc->busname.c_str(), nodeid, tpdo_cob_id, entry_name.c_str(), index, sub_index, offset);
					device.add_receive_pdo_mapping(tpdo_cob_id, entry_name, offset);

					set_entry_bridge(device, mc->busname, entry_name, BridgeType::publisher, SYNC_RATE / tpdo_interval);
				}

				offset += len;
			}


			if (m_node["setPdoMapping"] && m_node["setPdoMapping"].as<bool>()) {
				// entry num
				device.set_entry(0x1A00 + j, (uint8_t)0x0, (uint8_t)pdo_node.size(), 0, kaco::WriteAccessMethod::sdo);	
				// cob id
				device.set_entry(0x1800 + j, (uint8_t)0x1, (uint32_t)tpdo_cob_id, 0, kaco::WriteAccessMethod::sdo);
			}
		}
		else {
			RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] has no %s mapping, skipping.", mc->busname.c_str(), nodeid, tpdo_name.c_str());
		}


		if (m_node[rpdo_name]) {
			RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] set rpdo: %s", mc->busname.c_str(), nodeid, rpdo_name.c_str());
			YAML::Node pdo_node = m_node[rpdo_name];
			std::vector<kaco::Mapping> tpdo_mapping;
			tpdo_mapping.resize(0);
			uint8_t offset = 0;

			

			if (m_node["setPdoMapping"] && m_node["setPdoMapping"].as<bool>()) {
				// device.set_entry(0x1400 + j, (uint8_t)0x01, (uint32_t)rpdo_cob_id, kaco::WriteAccessMethod::sdo);
				device.set_entry(0x1400 + j, (uint8_t)0x01, 0x80000000 + (uint32_t)rpdo_cob_id, 0, kaco::WriteAccessMethod::sdo);
				device.set_entry(0x1600 + j, (uint8_t)0x0, (uint8_t)0x0, 0, kaco::WriteAccessMethod::sdo);
				device.set_entry(0x1400 + j, (uint8_t)0x02, (uint8_t)0x01, 0, kaco::WriteAccessMethod::sdo);
			}

			for (size_t k=0; k<pdo_node.size(); k++) {
				uint32_t 	value 		= pdo_node[k].as<int>();
				uint16_t 	index 		= (value >> 16) & 0xFFFF;
				uint8_t 	sub_index 	= (value >> 8) & 0xFF;
				uint8_t 	len 		= (value & 0xFF) >> 3;
				std::string entry_name 	= device.get_entry_name(index, sub_index);
				
				if (m_node["setPdoMapping"] && m_node["setPdoMapping"].as<bool>()) {
					RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] set_rpdo_mapping  index[%x] sub_index[%x] value[%x]", 
						mc->busname.c_str(), nodeid, 0x1600+j, k+1, value);
					device.set_entry(0x1600+j, k+1, value, 0, kaco::WriteAccessMethod::sdo);
				}

				set_entry_bridge(device, mc->busname, entry_name, BridgeType::subscriber);
				
				RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] add_transmit_pdo_mapping: cob_id[%x] name[%s] offset[%x]", 
					mc->busname.c_str(), nodeid, rpdo_cob_id, entry_name.c_str(), offset);
				tpdo_mapping.push_back({entry_name, offset});

				offset += len;
			}

			int rpdo_interval = 20;
			if (m_node["rpdoInterval"] && m_node["rpdoInterval"].as<int>() != 0) {
				rpdo_interval = m_node["rpdoInterval"].as<int>();
			}

			device.add_transmit_pdo_mapping(rpdo_cob_id, tpdo_mapping, kaco::TransmissionType::PERIODIC, std::chrono::milliseconds(rpdo_interval));

			if (m_node["setPdoMapping"] && m_node["setPdoMapping"].as<bool>()) {
				// entry num
				device.set_entry(0x1600 + j, (uint8_t)0x0, (uint8_t)pdo_node.size(), 0, kaco::WriteAccessMethod::sdo);	
				// cob id
				device.set_entry(0x1400 + j, (uint8_t)0x1, (uint32_t)rpdo_cob_id, 0, kaco::WriteAccessMethod::sdo);
			}
		}
		else {
			RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] has no %s mapping, skipping.", mc->busname.c_str(), nodeid, rpdo_name.c_str());
		}
	}


	if (m_node["sdoSettings"]) {
		YAML::Node settings_node = m_node["sdoSettings"];

		for(size_t k=0; k<settings_node.size(); k++) {
			uint64_t read_value = settings_node[k].as<long>();
			uint16_t index      = (read_value >> 40) & 0xFFFF;
			uint8_t  sub_index  = (read_value >> 32) & 0xFF;
			uint32_t value      = (read_value) & 0xFFFFFFFF;

			kaco::Value recovered_value = recover_value(device.get_entry_type(index, sub_index), value);
			// RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "entry type: %d  value type: %d", (int)device.get_entry_type(index, sub_index), (int)recovered_value.type);
			RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] sdo settings:  index[%x] sub_index[%x] value[%x]", 
				mc->busname.c_str(), nodeid, index, sub_index, recovered_value);
			device.set_entry(index, sub_index, recovered_value, 0, kaco::WriteAccessMethod::sdo);
		}
	}


	if (m_node["sdoInterface"]) {
		YAML::Node sdo_node = m_node["sdoInterface"];
		for (size_t k=0; k<sdo_node.size(); k++) {
			uint32_t 	value 		= sdo_node[k].as<int>();
			uint16_t 	index 		= (value >> 8) & 0xFFFF;
			uint8_t 	sub_index 	= value & 0xFF;
			std::string entry_name 	= device.get_entry_name(index, sub_index);
			
			RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] set sdo interface:  name[%s] index[%x] sub_index[%x] value[%x]", 
				mc->busname.c_str(), nodeid, entry_name.c_str(), index, sub_index, value);
			set_entry_bridge(device, mc->busname, entry_name, BridgeType::subscriber_sdo);
		}
	}
	else {
		RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "bus[%s] node[%d] has no sdoInterface, skipping", mc->busname.c_str(), nodeid);
	}

	std::this_thread::sleep_for(std::chrono::milliseconds(500));
	mc->master->core.nmt.send_nmt_message(nodeid, kaco::NMT::Command::start_node);
	return true;
}


void KaCanopen::set_entry_bridge(kaco::Device &d, const std::string busname, const std::string name, BridgeType type, double rate)
{
	if (type != BridgeType::publisher_timestamp) {
		switch(d.get_entry_type(name)) {
			case kaco::Type::uint8:
				d.set_entry(name, (uint8_t)0x0, kaco::WriteAccessMethod::cache);
				break;
			case kaco::Type::uint16:
				d.set_entry(name, (uint16_t)0x0, kaco::WriteAccessMethod::cache);
				break;
			case kaco::Type::uint32:
				d.set_entry(name, (uint32_t)0x0, kaco::WriteAccessMethod::cache);
				break;
			case kaco::Type::uint64:
				d.set_entry(name, (uint64_t)0x0, kaco::WriteAccessMethod::cache);
				break;
			case kaco::Type::int8:
				d.set_entry(name, (int8_t)0x0, kaco::WriteAccessMethod::cache);
				break;
			case kaco::Type::int16:
				d.set_entry(name, (int16_t)0x0, kaco::WriteAccessMethod::cache);
				break;
			case kaco::Type::int32:
				d.set_entry(name, (int32_t)0x0, kaco::WriteAccessMethod::cache);
				break;
			case kaco::Type::int64:
				d.set_entry(name, (int64_t)0x0, kaco::WriteAccessMethod::cache);
				break;	
			default:
				RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "unkown object data type[%d]", (int)d.get_entry_type(name));
				break;
		}
	}

	if (type == BridgeType::publisher) {
		auto pub = std::make_shared<kaco::EntryPublisher>(d, busname, name, kaco::ReadAccessMethod::cache);
		RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "set entry bridge, publisher: %s", name.c_str());
		bridge_->add_publisher(pub, rate);
	}
	else if (type == BridgeType::publisher_timestamp) {
		auto pub = std::make_shared<kaco::EntryTimestampPublisher>(d, busname, name, kaco::ReadAccessMethod::cache);
		bridge_->add_publisher(pub, rate);
	}
	else if (type == BridgeType::subscriber) {
		auto sub = std::make_shared<kaco::EntrySubscriber>(d, busname, name, kaco::WriteAccessMethod::cache);
		RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "set entry bridge, subscriber: %s", name.c_str());
		bridge_->add_subscriber(sub);
	}
	else if (type == BridgeType::subscriber_sdo) {
		auto sub = std::make_shared<kaco::EntrySubscriber>(d, busname, name, kaco::WriteAccessMethod::sdo);
		RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "set entry bridge, subscriber_sdo: %s", name.c_str());
		bridge_->add_subscriber(sub);
	}
}


kaco::Value KaCanopen::recover_value(kaco::Type type, uint32_t value)
{
	uint8_t ui8_data = 0;
	uint16_t ui16_data = 0;
	uint32_t ui32_data = 0;
	int8_t i8_data = 0;
	int16_t i16_data = 0;
	int32_t i32_data = 0;

	switch (type) {
		case kaco::Type::uint8:
			ui8_data = (uint8_t)(value & 0xFF);
			return ui8_data;
		case kaco::Type::uint16:
			ui16_data = (uint16_t)(value & 0xFFFF);
			return ui16_data;
		case kaco::Type::uint32:
			ui32_data = (uint32_t)(value & 0xFFFFFFFF);
			return ui32_data;
		case kaco::Type::int8:
			i8_data = (int8_t)(value & 0xFF);
			return i8_data;
		case kaco::Type::int16:
			i16_data = (int16_t)(value & 0xFFFF);
			return i16_data;
		case kaco::Type::int32:
			i32_data = (int32_t)(value & 0xFFFFFFFF);
			return i32_data;
		default:
			RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "unkown object data type[%d]", (int)type);
			break;
	}
	return 0;
}


void KaCanopen::send_sync_message(kaco::Master* master)
{
	kaco::Message msg;
	msg.cob_id = (uint16_t)0x80;
	msg.rtr = 0;
	msg.len = 0;

	master->core.send(msg);
}


void KaCanopen::sync_thread_func(std::atomic<bool>& running, kaco::Master* master)
{
	while (running) {
		send_sync_message(master);
		std::this_thread::sleep_for(std::chrono::milliseconds(1000 / SYNC_RATE));
	}
}


void KaCanopen::send_node_guard_message(kaco::Master* master, uint8_t node_id)
{
	kaco::Message msg;

	msg.cob_id = (uint16_t)0x700 + node_id;
	msg.rtr = 1;
	msg.len = 0;	    
  	master->core.send(msg);
}

void KaCanopen::node_guard_thread_func(std::atomic<bool>& running, kaco::Master* master, uint8_t node_id, uint16_t nodeGuarding_time)
{
	while (running) {
		send_node_guard_message(master, node_id);
		std::this_thread::sleep_for(std::chrono::milliseconds(nodeGuarding_time));
	}
}

int KaCanopen::resize_master_dev(std::string busname, kaco::Master* master)
{
	int num = 0;
	
	std::string manu_name;
	std::string param_prefix = busname + "/device_info";

	for (auto device_name : can_sys_info_.busInfos[busname].devices) {
    	if (device_name.compare(0, 6, "device") == 0) {
    		int device_type = 0;
    		if (bridge_->get_parameter(param_prefix + "/" + device_name + "/device_type", device_type)) {		
    			manu_name = get_manu_name_by_devicetype(device_type);		
			}
			
			if (dev_default_node_[manu_name]["standard"]) {
				if (dev_default_node_[manu_name]["standard"].as<bool>()) {
    				num++;
		    		RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "get standard device name: %s", device_name.c_str());	
    			}
    			else {
    				int nodeid = -1;
    				if (bridge_->get_parameter(param_prefix + "/" + device_name + "/node_id", nodeid)) {
	    				master->device_alive_callback(nodeid);
	    				RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "get custom device name: %s", device_name.c_str());			
    				}
    				else {
    					RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "cannot find custom device node id");
    				}
    			}
			}
			else
    			RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "%s standard key word not defined!", device_name.c_str());	
    	}
    }

    return num;
}


bool KaCanopen::set_canopen_entry_callback(const std::shared_ptr<agv_srvs::srv::SetCanopenEntry::Request> req, 
            std::shared_ptr<agv_srvs::srv::SetCanopenEntry::Response> resp)
{
	RCLCPP_INFO(rclcpp::get_logger("Kacanopen"), "get service call: busname[%s] nodeid[%d] index[0x%x] subindex[0x%x] value[%d]",
		req->bus_name.c_str(), req->node_id, req->index, req->sub_index, req->value);

	resp->success = true;
	for (auto m : masters_) {
		if (m->busname.compare(req->bus_name) == 0) {
			try {
				if (req->node_id == 0 && req->index == 0 && req->sub_index == 0) {
					switch (req->value) {
						case 1:
							m->master->core.nmt.broadcast_nmt_message(kaco::NMT::Command::start_node);
							break;
						default:
							break;
					}
				}
				else {
					kaco::Device& device = m->master->get_device_by_nodeid(req->node_id);
					kaco::Value recovered_value = recover_value(device.get_entry_type(req->index, req->sub_index), req->value);
					device.set_entry(req->index, req->sub_index, recovered_value, 0, kaco::WriteAccessMethod::sdo);	
				}
			}
			catch (...) {
				RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "service call failed.");
				resp->success = false;
			}
		}
	}

	return resp->success;
}

void KaCanopen::drivers_info_callback(const agv_msgs::msg::DriversInfo::ConstSharedPtr& msg)
{
	auto now = std::chrono::system_clock::now();
	last_drivers_info_time_ = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count() / 1000.0;
}


bool KaCanopen::check_devices_found_status(std::string busname, kaco::Master* master)
{
	std::vector<ParamNodeInfo> node_info = get_canopen_config_node_list(busname);
	std::vector<ParamNodeInfo> node_not_found;
	node_not_found.resize(0);

	for (auto pni : node_info) {
		try {
			kaco::Device& device = master->get_device_by_nodeid(pni.node_id);
		}
		catch(...) {
			node_not_found.push_back(pni);
		}
	}

	auto now = std::chrono::system_clock::now();
	auto now_timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count() / 1000.0;

	if (node_not_found.size() > 0) {
		for (auto n_pni : node_not_found) {
			agv_msgs::msg::AGVEvent event;
	    	event.header.stamp = bridge_->now();
	    	event.header.frame_id = bridge_->get_name();
			event.ecode = agv_msgs::msg::AGVEvent::CANOPEN_SERVO_ERROR_EVENT;
			event.type = agv_msgs::msg::AGVEvent::ALARM;
			event.action = agv_msgs::msg::AGVEvent::ESTOP;
			event.ackreq = true;
			event.param1 = n_pni.node_id;
			event.param2 = 0;
			event.eventtime = now_timestamp;
			char desc[128];
			snprintf(desc, sizeof(desc), "CANOPEN NODE NOTFOUND %s %s node_id[%d] manufacturer[%s] relative_joint[%s]", 
				busname.c_str(), n_pni.device_name.c_str(), n_pni.node_id, n_pni.manu_name.c_str(), n_pni.joint_name.c_str());
			event.description = std::string(desc);
			RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), "%s", desc);
			event_pub_->publish(event);
		}

		return false;
	}
	else {
		agv_msgs::msg::AGVEvent event;
    	event.header.stamp = bridge_->now();
    	event.header.frame_id = bridge_->get_name();
		event.ecode = agv_msgs::msg::AGVEvent::NONE_EVENT;
		event.type = agv_msgs::msg::AGVEvent::INFO;
		event.action = agv_msgs::msg::AGVEvent::NONE;
		event.ackreq = true;
		event.param1 = 0;
		event.param2 = 0;
		event.eventtime = now_timestamp;
		event.description = "No event";
		event_pub_->publish(event);
	}

	return true;
}


void KaCanopen::check_devices_alive_status()
{
	auto now = std::chrono::system_clock::now();
	auto now_timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count() / 1000.0;
	double last_update_time = 0.0;
	int nodeid = 0;

	bool event_happened = false;

	for (auto& mc : masters_) {
		for (auto& cns : mc->node_status.node_status) {
			try {
				kaco::Device& device = mc->master->get_device_by_nodeid(cns.nodeid);
			}
			catch(...) {
				cns.communication_status = std::string("NOTFOUND");
				agv_msgs::msg::AGVEvent event;
		    	event.header.stamp = bridge_->now();
		    	event.header.frame_id = bridge_->get_name();
				event.ecode = agv_msgs::msg::AGVEvent::CANOPEN_SERVO_ERROR_EVENT;
				event.type = agv_msgs::msg::AGVEvent::ALARM;
				event.action = agv_msgs::msg::AGVEvent::ESTOP;
				event.ackreq = true;
				event.param1 = cns.nodeid;
				event.param2 = 0;
				event.eventtime = now_timestamp;
				char desc[128];
				snprintf(desc, sizeof(desc), "CANOPEN NODE NOTFOUND %s node_id[%d]", mc->busname.c_str(), cns.nodeid);
				event.description = std::string(desc);
				RCLCPP_ERROR_THROTTLE(bridge_->get_logger(), *(bridge_->get_clock()), 1.0, "%s", desc);
				ep_.add(event, 1, cns.nodeid);
				continue;
			}

			ep_.clear(1, cns.nodeid);

			last_update_time = mc->master->get_device_by_nodeid(cns.nodeid).get_alive_timestamp();
			cns.timelost = now_timestamp - last_update_time;

			if (cns.timelost > timeout_) {
				cns.communication_status = std::string("LOST");
	    		agv_msgs::msg::AGVEvent event;
		    	event.header.stamp = bridge_->now();
		    	event.header.frame_id = bridge_->get_name();
				event.ecode = agv_msgs::msg::AGVEvent::CANOPEN_SERVO_ERROR_EVENT;
				event.type = agv_msgs::msg::AGVEvent::ALARM;
				event.action = agv_msgs::msg::AGVEvent::ESTOP;
				event.ackreq = true;
				event.param1 = 100 + cns.nodeid;
				event.param2 = (int)cns.timelost;
				event.eventtime = now_timestamp;
				char desc[128];
				snprintf(desc, sizeof(desc), "CANOPEN NODE TIMEOUT bus[%s]  nodeid[%d] last_update_time: %.3f  from now: %.3f", 
								mc->busname.c_str(), cns.nodeid, last_update_time, cns.timelost);
				event.description = std::string(desc);
				RCLCPP_ERROR_THROTTLE(bridge_->get_logger(), *(bridge_->get_clock()), 1.0, "%s", desc);
				ep_.add(event, 1, 100 + cns.nodeid);
			}
			else {
				cns.communication_status = std::string("CONNECTED");
				ep_.clear(1, 100 + cns.nodeid);
			}
		}
	}

	// Time reversal
	if (now_timestamp < last_system_time_) {
		RCLCPP_ERROR_THROTTLE(bridge_->get_logger(), *(bridge_->get_clock()), 
				1.0, "SYSTEM TIME REVERSAL !!! now: %.3f  last: %.3f", now_timestamp, last_system_time_);
		time_reversal_detect_ = true;
	}
	else {
		time_reversal_detect_ = false;
	}

	// Driver node lost
	if (abs(now_timestamp - last_drivers_info_time_) > 1.0 && abs(now_timestamp - node_start_time_) > 180.0) {
		RCLCPP_ERROR_THROTTLE(bridge_->get_logger(), *(bridge_->get_clock()), 1.0, "DRIVER NODE LOST !!!");
		driver_lost_detect_ = true;
	}
	else {
		driver_lost_detect_ = false;
	}

	if (time_reversal_detect_ || driver_lost_detect_) {
		event_happened = true;
 
		agv_msgs::msg::AGVEvent event;
    	event.header.stamp = bridge_->now();
    	event.header.frame_id = bridge_->get_name();
		event.ecode = agv_msgs::msg::AGVEvent::CANOPEN_SERVO_ERROR_EVENT;
		event.type = agv_msgs::msg::AGVEvent::ALARM;
		event.action = agv_msgs::msg::AGVEvent::ESTOP;
		event.ackreq = true;
		event.eventtime = now_timestamp;

		if (time_reversal_detect_) {
			event.param1 = 201;
			event.param2 = (int)last_system_time_;
			char desc[128];
			snprintf(desc, sizeof(desc), "TIME REVERSAL now: %.3f  last_system_time: %.3f", 
							now_timestamp, last_system_time_);
			event.description = std::string(desc);
			ep_.add(event, 1, 201);
		}
		else
			ep_.clear(1, 201);

		if (driver_lost_detect_) {
			event.param1 = 202;
			event.param2 = (int)last_drivers_info_time_;
			char desc[128];
			snprintf(desc, sizeof(desc), "DRIVER NODE LOST now: %.3f  last_drivers_info_time: %.3f", 
							now_timestamp, last_drivers_info_time_);
			event.description = std::string(desc);
			ep_.add(event, 1, 202);
		}
		else 
			ep_.clear(1, 202);

 		if (abs(now_timestamp - last_set_zero_time_) > 0.5) {
 			last_set_zero_time_ = now_timestamp;
 			need_clear_force_ = true;
 			set_vels_zero(true);
 		}
	}
	else {
		ep_.clear(1, 201);
		ep_.clear(1, 202);
	}

	if (!time_reversal_detect_ && !driver_lost_detect_ && need_clear_force_) {
		set_vels_zero(false);
		need_clear_force_ = false;
	}

	if (!time_reversal_detect_) {
		last_system_time_ = now_timestamp;
	}
}

void KaCanopen::set_vels_zero(bool force)
{
	int flag = force ? 1 : 2;
	if (flag == 1) {
		RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "set force zero.");
	}
	else if (flag == 2) {
		RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "clear force zero.");
	}

	kaco::Value recovered_value;

	for (auto m : masters_) {
		for (auto cns : m->node_status.node_status) {
			if (cns.communication_status == std::string("CONNECTED")) {
				try {
					kaco::Device& device_temp = m->master->get_device_by_nodeid(cns.nodeid);
				}
				catch (...) {
					RCLCPP_ERROR(rclcpp::get_logger("Kacanopen"), 
						"set_vels_zero: cannot find device nodeid[%d]", cns.nodeid);
				}

				kaco::Device& device = m->master->get_device_by_nodeid(cns.nodeid);
			
				switch (device.get_device_type()) {
					case DeviceManufacturer::KINCO:
						RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "set node[%d] KINCO vel 0", device.get_node_id());
						recovered_value = recover_value(device.get_entry_type(0x60ff, 0x00), 0x00);
						device.set_entry(0x60ff, 0x00, recovered_value, flag, kaco::WriteAccessMethod::cache);		// target vel
						break;
					case DeviceManufacturer::ZAPI:
						RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "set node[%d] ZAPI vel 0", device.get_node_id());
						recovered_value = recover_value(device.get_entry_type(0x3001, 0x00), 0x00);
						device.set_entry(0x3001, 0x00, recovered_value, flag, kaco::WriteAccessMethod::cache);	// target vel
						recovered_value = recover_value(device.get_entry_type(0x3002, 0x00), 0x07);
						device.set_entry(0x3002, 0x00, recovered_value, flag, kaco::WriteAccessMethod::cache);	// control word
						break;
					case DeviceManufacturer::CURTIS:
						RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "set node[%d] CURTIS vel 0", device.get_node_id());
						recovered_value = recover_value(device.get_entry_type(0x3000, 0x00), 0x03);
						device.set_entry(0x3000, 0x00, recovered_value, flag, kaco::WriteAccessMethod::cache);	// control word
						recovered_value = recover_value(device.get_entry_type(0x3001, 0x00), 0x00);
						device.set_entry(0x3001, 0x00, recovered_value, flag, kaco::WriteAccessMethod::cache);	// target vel
						recovered_value = recover_value(device.get_entry_type(0x3002, 0x00), 0x00);
						device.set_entry(0x3002, 0x00, recovered_value, flag, kaco::WriteAccessMethod::cache);	// acc
						recovered_value = recover_value(device.get_entry_type(0x3003, 0x00), 0x02);
						device.set_entry(0x3003, 0x00, recovered_value, flag, kaco::WriteAccessMethod::cache);	// dec
						break;
					case DeviceManufacturer::YUFENG_CURTIS:
						RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "set node[%d] YUFENG_CURTIS vel 0", device.get_node_id());
						recovered_value = recover_value(device.get_entry_type(0x3000, 0x00), 0x00);
						device.set_entry(0x3000, 0x00, recovered_value, flag, kaco::WriteAccessMethod::cache);	// target vel
						recovered_value = recover_value(device.get_entry_type(0x300a, 0x00), 0x00);
						device.set_entry(0x300a, 0x00, recovered_value, flag, kaco::WriteAccessMethod::cache);	// acc
						recovered_value = recover_value(device.get_entry_type(0x300b, 0x00), 0x01);
						device.set_entry(0x300b, 0x00, recovered_value, flag, kaco::WriteAccessMethod::cache);	// dec
						break;
					case DeviceManufacturer::KEYA:
						RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "set node[%d] KEYA vel 0", device.get_node_id());
						recovered_value = recover_value(device.get_entry_type(0x6081, 0x00), 0x00);
						device.set_entry(0x6081, 0x00, recovered_value, flag, kaco::WriteAccessMethod::cache);	// A target vel
						recovered_value = recover_value(device.get_entry_type(0x6081, 0x00), 0x00);
						device.set_entry(0x6084, 0x00, recovered_value, flag, kaco::WriteAccessMethod::cache);	// B target vel
						break;
					case DeviceManufacturer::NEWSTART:
						RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "set node[%d] NEWSTART vel 0", device.get_node_id());
						recovered_value = recover_value(device.get_entry_type(0x60ff, 0x00), 0x00);
						device.set_entry(0x60ff, 0x00, recovered_value, flag, kaco::WriteAccessMethod::cache);	// target vel
						break;
					case DeviceManufacturer::NEWSTART_MAIRUI:
						RCLCPP_WARN(rclcpp::get_logger("Kacanopen"), "set node[%d] NEWSTART_MAIRUI vel 0", device.get_node_id());
						recovered_value = recover_value(device.get_entry_type(0x60ff, 0x00), 0x00);
						device.set_entry(0x60ff, 0x00, recovered_value, flag, kaco::WriteAccessMethod::cache);	// target vel
						break;
					default:
						break;
				}
			}
		}
	}
}


int main(int argc, char** argv) {
    rclcpp::init(argc, argv);

    try {
        auto bridge = std::make_shared<kaco::Bridge>();
        auto kac = std::make_unique<KaCanopen>();

        std::atomic<bool> running{true};
	    rclcpp::on_shutdown([&running]() {
	        running = false;
	    });

        if (!kac->init(bridge)) {
            throw std::runtime_error("Failed to initialize KaCanopen");
        }

        rclcpp::executors::MultiThreadedExecutor executor;
        executor.add_node(bridge);

        std::thread spin_thread([&executor, &running]() {
            if (running && rclcpp::ok()) {
                executor.spin();
            }
        });

        rclcpp::Rate rate(20);
        while (running && rclcpp::ok()) {
            try {
                kac->update();
                rate.sleep();
            } catch (const std::exception& e) {
                RCLCPP_ERROR(bridge->get_logger(), 
                    "Error in main loop: %s", e.what());
                running = false;
                break;
            }
        }

        RCLCPP_INFO(bridge->get_logger(), "Shutting down...");
        running = false;
        executor.cancel();
        
        if (spin_thread.joinable()) {
            spin_thread.join();
        }

        bridge.reset();

    } catch (const std::exception& e) {
        throw std::runtime_error("Fatal error:" + std::string(e.what()));
        return 1;
    }

    rclcpp::shutdown();
    return 0;
}